/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package hciproject;

/**
 *
 * @author Alerk
 */
public class Trajectory {

    public int length;
    public float[] data;
    private final double VERY_BIG = 100000000;
    public final static int ROLL = 1;
    public final static int PITCH = 2;
    public final static int YAW = 3;

    Trajectory(int aLength) {
        this.length = aLength;
        data = new float[aLength];
    }

    public void setData(float[] aData) {
        if (aData.length == length) {
            for (int i = 0; i < length; i++) {
                data[i] = aData[i];
            }
        } else {
            this.length = aData.length;
            for (int i = 0; i < length; i++) {
                data[i] = aData[i];
            }
        }
    }

    public float[] getData() {
        return this.data;
    }

    public void setLength(int aLength)
    {
        this.length = aLength;
    }
    public int getLength()
    {
        return this.length;
    }

    public double getWarpingPath(Trajectory input, Trajectory output) {
        //ofstream debug_file(".//KeonHong//debug_file.dat");
        //ofstream glob(".//KeonHong//glob.dat");
        //ofstream output_file(".//KeonHong//output_file.dat");
        double [][]globdist;
        double [][]Dist;
        //Hashtable ^trainedData = gcnew Hashtable();


        double top, mid, bot, cheapest, total=0;
        int [][]move;
        int [][]warp;
        int [][]temp;
        int I, X, Y, n, i, j, k;

        int xsize = this.length;
        int ysize = input.getLength();

        /* allocate memory for Dist */
        if ((Dist = new double[xsize][ysize]) == null) {
            // debug_file << "Memory allocation error (Dist)" << endl;
        }
        /* allocate memory for globdist */
        if ((globdist = new double[xsize][ysize]) == null) {
            //debug_file << "Memory allocation error (globdist)" << endl;
        }
        /* allocate memory for move */

        //if ((move = (short *)malloc(xsize * sizeof(short *))) == NULL)
        if ((move = new  int[xsize][ysize])== null)
        {
            //debug_file << "Memory allocation error (move)" << endl;
        }
        /* allocate memory for temp */

        //if ((temp = (short *)malloc(xsize * 2 * sizeof(short *))) == NULL)
        if ((temp = new  int [2*xsize][2]) == null)
        {
            //debug_file << "Memory allocation error (temp)" << endl;
        }
        /* allocate memory for warp */

        //if ((warp = (short *)malloc(xsize * 2 * sizeof(short *))) == NULL)
        if ((warp = new  int[xsize*2][2]) == null)
        {
            // debug_file << "Memory allocation error (warp)" << endl;
        }
        /*Compute distance matrix*/

        for (i = 0; i < xsize; i++)
        {
            for (j = 0; j < ysize; j++)
            {
                Dist[i][j] = (data[i] - input.getData()[j]) * (data[i] - input.getData()[j]);
                //total = total + (data[i] - input.getData()[j]) * (data[i] - input.getData()[j]);
               // for (k=0;k<params;k++) {
                //  total = total + ((data[i][k] - y[j][k]) * (x[i][k] - y[j][k]));
               // }

               // Dist[i][j] = total;
            }
        }

        /*% for first frame, only possible match is at (0,0)*/

        globdist[0][0] = Dist[0][0];
        for (j = 1; j < xsize; j++) {
            globdist[j][0] = VERY_BIG;
        }

        globdist[0][1] = VERY_BIG;
        globdist[1][1] = globdist[0][0] + Dist[1][1];
        move[1][1] = 2;

        for (j = 2; j < xsize; j++) {
            globdist[j][1] = VERY_BIG;
        }

        for (i = 2; i < ysize; i++) {
            globdist[0][i] = VERY_BIG;
            globdist[1][i] = globdist[0][i - 1] + Dist[1][i];
            //if (debug = 1)
            //debug_file << "globdist[2][" << i << "] = " << globdist[2][i] << endl;
            //  fprintf(debug_file,"globdist[2][%d] = %.2e\n",i,globdist[2][i]);

            for (j = 2; j < xsize; j++) {
                top = globdist[j - 1][i - 2] + Dist[j][i - 1] + Dist[j][i];
                mid = globdist[j - 1][i - 1] + Dist[j][i];
                bot = globdist[j - 2][i - 1] + Dist[j - 1][i] + Dist[j][i];
                if ((top < mid) && (top < bot)) {
                    cheapest = top;
                    I = 1;
                } else if (mid < bot) {
                    cheapest = mid;
                    I = 2;
                } else {
                    cheapest = bot;
                    I = 3;
                }

                /*if all costs are equal, pick middle path*/
                if ((top == mid) && (mid == bot)) {
                    I = 2;
                }

                globdist[j][i] = cheapest;
                move[j][i] = I;
                //debug_file << "move[" << j << "," << i << "]=" << move[j][i] ;
                //debug_file <<"globdist[" << j << "]["<< i << "] = " << globdist[j][i] << endl;
                //debug_file <<"move j:"<< j << ":i:" << i << " = " << move[j][i] << endl;
                //if (debug == 1) {
                //  fprintf(debug_file,"globdist[%d][%d] = %.2e\n",j,i,globdist[j][i]);
                //  fprintf(debug_file,"move j:%d:i:%d=%d\n",j,i,move[j][i]);
                //      }
            }
            //debug_file << endl;
        }

        for (j = 0; j < xsize; j++) {
            for (i = 0; i < ysize; i++) {
                //fprintf(debug_file,"[%d %d] globdist: %.2e    move: %d    \n",j,i,globdist[j][i],move[j][i]);
                //debug_file << "[" << j << " " << i << "] globdist: " << globdist[j][i] << "    move: " << move[j][i] << endl;
            }
        }

        X = ysize - 1;
        Y = xsize - 1;
        n = 0;
        warp[n][0] = X;
        warp[n][1] = Y;


        while (X > 0 && Y > 0) {
            n = n + 1;


            if (n > ysize * 2) {
                //fprintf (stderr,"Warning: warp matrix too large!");
                //debug_file << "Warning: warp matrix too large!";
                //exit(1);
            }

            // if (debug == 1)
            //   fprintf(debug_file,"Move %d %d %d\n", Y, X, move[Y][X]);
            //debug_file << "Move " << Y << " " << X << " " << move[Y][X] << endl;

            if (move[Y][X] == 1) {
                warp[n][0] = X - 1;
                warp[n][1] = Y;
                n = n + 1;
                X = X - 2;
                Y = Y - 1;
            } else if (move[Y][X] == 2) {
                X = X - 1;
                Y = Y - 1;
            } else if (move[Y][X] == 3) {
                warp[n][0] = X;
                warp[n][1] = Y - 1;
                n = n + 1;
                X = X - 1;
                Y = Y - 2;
            } 
            //else {
                //fprintf(stderr,"Error: move not defined for X = %d Y = %d\n",X,Y);
                //debug_file << "Error: move not defined for X = " << X << " Y = " << Y << endl;
            //}
            warp[n][0] = X;
            warp[n][1] = Y;

        }


        /*flip warp*/
        for (i = 0; i <= n; i++) {
            temp[i][0] = warp[n - i][0];
            temp[i][1] = warp[n - i][1];

        }

        for (i = 0; i <= n; i++) {
            warp[i][0] = temp[i][0];
            warp[i][1] = temp[i][1];

        }
        
        //CHANGE Y TRAJECTORY
        //Trajectory output = new Trajectory(xsize);
        
        int index, counter;
        index = 0;
        counter = 1;
        //for(i=0;i<500;i++)
        //	 y1->data[i]=0;
        output.data[index] = input.data[warp[0][0]];
        //output_file << "                      counter      index          i     warp[i][1]  warp[i][0]" << endl;
        //output_file << "                      " << counter << "            " << index << "              " << 0 << "     " << warp[0][1] << "           " << warp[0][0] << endl;
        for (i = 1; i <= n; i++) 
        {
            if (warp[i][1] == warp[i - 1][1]) 
            {
                //continue add
            	output.data[index] += input.data[warp[i][0]];
                counter++;
                //output_file << " ==                    " << counter << "      " << index << "(y1["<<index<<"]= " << y1->data[index] << "     " << i << "     " << warp[i][1] << "           " << warp[i][0] << endl;
            } else {
                //refresh
            	output.data[index++] /= counter;
                //output_file << " != : y1[" << index-1 << "]=" << y1->data[index-1];
            	output.data[index] = input.data[warp[i][0]];
                counter = 1;
                //output_file << "   " << counter << "      " << index << "(y1["<<index<<"]= " << y1->data[index] << "     " << i  << "     " << warp[i][1] << "           " << warp[i][0] << endl;
            }
        }
        output.data[index] /= counter;        
//        delete(Dist);
//        delete(globdist);
//        delete(temp);
//        delete(move);
//        delete(warp);
        return globdist[xsize-1][ysize-1];        
    }
}
